#pragma once
#include "rtdev/rtdev.h"
#include <string>

namespace rtdev {

class ServoItf : public RtDevItf
{
public:
    enum class TRACKING_MODE
    {
        OPMODE_CSP = 8,
        OPMODE_CST = 10,
        OPMODE_HOME = 6,
    };
    struct DeviceInfo
    {
        int bus_id;
        int dev_id;
        int dev_type;  //0:jaka joint servo 1: standard cia402 servo
    };

public:
    ServoItf();
    ~ServoItf() override;
    virtual int init(std::string bus_name, int devid, int joint_id);
    virtual void set_sim(bool sim);
    int reset();
    virtual int bind_bus(std::string bus_name, int devid);  //绑定总线
    virtual int set_opmode(TRACKING_MODE mode);             //设置伺服模式

    // virtual int get_opmode(bool is_cmd);                               //获取伺服模式反馈或指令
    virtual int set_position(double pos);                                 //位置模式下，设置伺服位置
    virtual double get_position(bool is_cmd);                             //获取伺服位置反馈或指令
    virtual int set_torq_offset(int16_t torq_off);                        //设置伺服力矩前馈
    virtual int set_torq(double torq);                                    //转矩模式下，设置伺服力矩
    virtual double get_torq();                                            //获取伺服力矩反馈
    virtual std::vector<uint32_t> get_errorcode();                        //获取伺服报错码
    virtual int set_controlword(uint16_t cw);                             //设置伺服控制字
    virtual uint16_t get_controlword();                                   //获取伺服控制字
    virtual uint16_t get_statusword();                                    //获取伺服状态字
    virtual uint8_t get_opmode_display();                                 //获取当前伺服所处控制模式
    bool is_alive() final;                                                //伺服是否存活
    bool is_sim() final;                                                  //是否为仿真
    virtual bool is_homing();                                             // 是否处于回零模式
    virtual int set_opt(std::string opt_name, void* data, size_t s) = 0;  //设置特殊选项

    bool cmd_enable_ = false;
    virtual void set_enable(bool enable) { cmd_enable_ = enable; };
    virtual void set_home();

    virtual int get_sdo_blocking(uint32_t objid, void* data, int s, int retry) = 0;
    virtual int set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry) = 0;
    virtual int set_send_data(int busid) = 0;  //通过bus接口设置发送数据
    virtual int get_recv_data(int busid) = 0;  //通过bus接口获取接收数据

    int id() const { return cfg_.devid; }

    virtual bool is_enable() = 0;
    virtual bool is_homed() = 0;
    virtual uint32_t get_act_collision_lv() = 0;
    virtual bool is_tor_overload() = 0;
    virtual bool is_backdrive() = 0;
    virtual bool is_collision() = 0;

protected:
    void check_alive() final;         //检查伺服是否存活,每个周期调用检测
    int keep_alive(int alive) final;  //保持伺服存活，每个周期调用

protected:
    std::shared_ptr<rtbus::RtCommuItf> bus_;

    bool sim_flag_;
    int alive_watchdog_;
    bool alive_flag_;
    int last_alive_cnt_;

    struct SyncCmd  //周期性指令
    {
        uint16_t controlword;
        int32_t target_pos;   //only for csp mode
        int16_t target_torq;  //only for cst mode
        int16_t torq_offset;
        uint8_t opmode;
        bool error_reset;
        int collision_reset;
        bool home;
    };
    struct SyncStat  //周期性反馈
    {
        bool enable;

        uint16_t statusword;
        int32_t act_pos;
        int16_t act_torq;
        uint8_t opmode_display;
        uint32_t errorcode;
        uint8_t act_collision_lv;
        bool torq_overload;
        bool error_sta;
        bool home;

        //ota data
        int16_t ota_pos_diff;
        int16_t ota_dual_encoder_diff;
        int16_t ota_dc_vol;
    };

    struct Config
    {
        std::string bus;
        int devid;

        int joint_id;  // 关联的axis的ID号
    } cfg_ = {};

public:
    SyncCmd cmd_ = {};
    SyncStat stat_ = {};
};

std::shared_ptr<ServoItf> create_servo(std::string busname);

}  // namespace rtdev
